package com.lego.jbv;

import lejos.nxt.*;
import lejos.util.Delay;
/*
 * Avoid behavior
 */

class Escape extends Thread
{
    private SharedCar car = new SharedCar();

	private int power = 70, ms = 500;
	TouchSensor bumperLeft = new TouchSensor(SensorPort.S1);
	TouchSensor bumperRight = new TouchSensor(SensorPort.S4);
	
    public Escape(SharedCar car)
    {
       this.car = car;
    }
    
	public void run() 
    {				       
        while (true)
        {
        	if(bumperLeft.isPressed() || bumperRight.isPressed())
        	{
        		LCD.clear();
        		LCD.drawString("Bumper activated", 0, 0);
        		car.stop();
        	
	        	if(bumperLeft.isPressed() && bumperRight.isPressed())
	        	{
	        		car.backward(power, power);
	    	    	Delay.msDelay(ms);
	    	    	car.backward(0, power);
	    	    	Delay.msDelay(2*ms);
	        	}
	        	
	        	else if(bumperLeft.isPressed())
	        	{
	        		car.backward(power, power);
	    	    	Delay.msDelay(ms);
	    	    	car.backward(power, 0);
	    	    	Delay.msDelay(2*ms);
	        	}
	        	
	        	else if(bumperRight.isPressed())
	        	{
	        		car.backward(power, power);
	    	    	Delay.msDelay(ms);
	    	    	car.backward(0, power);
	    	    	Delay.msDelay(2*ms);
	        	}
	        	
	        	LCD.clear();
        	}
        	
        	else
        	{
        		Delay.msDelay(ms);
	        	car.noCommand();
        	}
        	
	    	// Monitor the distance in front of the car and stop
	    	// when an object gets to close
        	/*frontDistance = sonar.getDistance();
	    	while ( frontDistance >= stopThreshold )
	    	{
	    		car.noCommand();
	    		frontDistance = sonar.getDistance();
	    	}
	    	car.stop();
	    	Delay.msDelay(ms);
	    	
	    	// Get the distance to the left
	    	car.forward(0, power);
	    	Delay.msDelay(ms);
	    	leftDistance = sonar.getDistance();
    	
	    	// Get the distance to the right
	    	car.backward(0, power);
	    	Delay.msDelay(ms);
	    	car.forward(power, 0);
	    	Delay.msDelay(ms);
	    	rightDistance = sonar.getDistance();
	    	
	    	// Turn in the direction with most space in front of the car
	    	if ( leftDistance > rightDistance ){
		    	car.backward(power, 0);
		    	Delay.msDelay(ms);
		    	car.forward(0, power);
		    	Delay.msDelay(ms);
	    	}*/
        }
    }
}

